#include "DiscMagnet.h"
#include "../CommandBase.h"
#include "../NetworkMap.h"
#include "../Robotmap.h"

DiscMagnet::DiscMagnet() : Subsystem("DiscMagnet")
, discMagnetSolenoid(DISC_MAGNET_SOLENOID)
,discMagnetVictor(DISC_MAGNET_VICTOR){
	isUp = true;
	sweeperStart = new Notifier(TimerEventHandler(DiscMagnet::StartSweeper), (void*) this);
}
    
void DiscMagnet::InitDefaultCommand() {
	// Set the default command for a subsystem here.
}

void DiscMagnet::StartSweeper(void * handle)
{
	DiscMagnet* dm = (DiscMagnet*) handle;
	dm->discMagnetVictor.Set(1.0);
}
bool DiscMagnet::MagnetIsUp()
{
	return isUp;
}
void DiscMagnet::DiscMagnetControl(discMagnetState State){
	
	if(State == kDiscMagnetDown){
		isUp = false;
		sweeperStart->StartSingle(0.25);
		discMagnetSolenoid.Set(true);
		discMagnetVictor.Set(Relay::kForward);
	} else if(State == kDiscMagnetUp){
		isUp = true;
		discMagnetSolenoid.Set(false);
		discMagnetVictor.Set(0.0);
	} else{
		isUp = true;
		discMagnetVictor.Set(Relay::kOff);
		discMagnetSolenoid.Set(0.0);
	}
	CommandBase::client->SetNWInt(swprSol, (int)discMagnetSolenoid.Get());
	CommandBase::client->SetNWFloat(swprRelay, (int)discMagnetVictor.Get());
}

// Put methods for controlling this subsystem
// here. Call these from Commands.
